#include <vector>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <map>
using namespace std;
class Generator_queue
{
private:
    std::map<std::string,std::vector<Eigen::Vector3d>> formation_queue;
public:
    Generator_queue(/* args */){}
    ~Generator_queue(){}

    std::map<std::string,std::vector<Eigen::Vector3d>> get_queue(){
        return formation_queue;
    }


    std::vector<Eigen::Vector3d> generate_line_formation( Eigen::Vector3d start_point = Eigen::Vector3d(0,0, 0)) {
        std::vector<Eigen::Vector3d> positions;

        for (int i = 0; i < num_uavs; ++i) {
            Eigen::Vector3d pos = start_point + Eigen::Vector3d(i * spacing, 0,height);
            std::cout << "line: " << pos.transpose() << std::endl;
            positions.push_back(pos);
        }
        
        return positions;
    }

    // 生成矩形队形
    std::vector<Eigen::Vector3d> generate_rectangle_formation(int rows, int cols,Eigen::Vector3d start_point = Eigen::Vector3d(0,0, 0)) {
        cols = (num_uavs / rows) + 1;
        
        std::vector<Eigen::Vector3d> positions;
        int n = 0;
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                Eigen::Vector3d pos = start_point + Eigen::Vector3d(i * spacing, j * spacing,height);
                std::cout << "rectangle: " << pos.transpose() << std::endl;
                positions.push_back(pos);
                n++;
                if (n == num_uavs) break;
            }
            if (n == num_uavs) break;
        }
        std::cout << "rectangle: " << positions.size() << std::endl;
        return positions;
    }

    // 生成圆形队形
    std::vector<Eigen::Vector3d> generate_circle_formation(double radius = 10, Eigen::Vector3d center = Eigen::Vector3d(0,0, 0)) {
        std::vector<Eigen::Vector3d> positions;
        Eigen::Vector3d centers = center + Eigen::Vector3d(0, 0,height);
        positions.push_back(centers);
        std::cout << "arrow: " << centers.transpose() << std::endl;
        for (int i = 1; i < num_uavs; ++i) {
            double angle = 2 * M_PI * i / num_uavs;
            Eigen::Vector3d pos = center + Eigen::Vector3d(radius * std::cos(angle), radius * std::sin(angle),height);
            std::cout << "circle: " << pos.transpose() << std::endl;
            positions.push_back(pos);
        }
        std::cout << "circle: " << positions.size() << std::endl;
        return positions;
    }

    // 生成箭头队形
    std::vector<Eigen::Vector3d> generate_arrow_formation(Eigen::Vector3d start_point = Eigen::Vector3d(0,0, 0), double angle = M_PI / 4) {
        std::vector<Eigen::Vector3d> positions;
        Eigen::Vector3d start_points = start_point + Eigen::Vector3d(0, 0,height);
        positions.push_back(start_points);  
         std::cout << "arrow: " << start_points.transpose() << std::endl;
        int n = 1;
        int rows = (num_uavs - 1) / 2 +1;  // Rows on each side of the leader

        for (int i = 1; i <= rows; ++i) {
            double dx = i * spacing;
            double dy = i * spacing * std::tan(angle);

            // Add two positions symmetrically around the leader
            Eigen::Vector3d pos1 =start_point + Eigen::Vector3d(dx, dy,height);
            Eigen::Vector3d pos2 =start_point + Eigen::Vector3d(dx, -dy,height);
            std::cout << "arrow: " << pos1.transpose() << std::endl;
            std::cout << "arrow: " << pos2.transpose() << std::endl;
            positions.push_back(pos1);
            n++;
            if (n == num_uavs) break;
            positions.push_back(pos2);
            n++;
            if (n == num_uavs) break;
        }
        std::cout << "arrow: " << positions.size() << std::endl;
        return positions;
    }

    // origin 队形
    std::vector<Eigen::Vector3d> Generator_origin_formation(Eigen::Vector3d start_point = Eigen::Vector3d(0,0, 0),double distance = 6.0){
        std::vector<Eigen::Vector3d> positions;
        Eigen::Vector3d start_points = start_point + Eigen::Vector3d(0, 0,height);
        std::cout << "origin: " << start_points.transpose() << std::endl;
        positions.push_back(start_points);
        int n = 1;
        for (size_t i = 1; i < num_uavs *2; i++)
        {
           
            int id = (i+1)/2;
            Eigen::Vector3d pos1 = start_point +  Eigen::Vector3d(id * -1.0 *distance, 0,height);
            std::cout << "origin: " << pos1.transpose() << std::endl;
            positions.push_back(pos1);
            n++;
            if (n == num_uavs) break;
            Eigen::Vector3d pos2 = start_point +  Eigen::Vector3d(id * 1.0 *distance, 0,height);
            std::cout << "origin: " << pos2.transpose() << std::endl;
            positions.push_back(pos2);
            n++;
            if (n == num_uavs) break;
            i += 1;
           
        }
        std::cout << "origin: " << positions.size() << std::endl;
        return positions;
        
    }

    void set_property(int num_uavs, double height = 0.0, double spacing = 10.0) {
        this->num_uavs = num_uavs;
        this->height = height;
        this->spacing = spacing;
    }
    void generate_formation() {
        formation_queue.clear();
        formation_queue["arrow"] = generate_arrow_formation();
        formation_queue["line"] = generate_line_formation();
        formation_queue["rectangle"] = generate_rectangle_formation(2, 3);
        formation_queue["circle"] = generate_circle_formation();
        formation_queue["origin"] = Generator_origin_formation();


    }

    int num_uavs = 3;
    double height = 0.0;
    double spacing = 5.0;

};
